import numpy as np
import matplotlib.pyplot as plt
import pandas as pd
from scipy.integrate import quad

p = 0.55  # 间距长度,单位m
v_head = 1.0  # 头部速度

det_t=0.1              #时间间隙
times=int(1/det_t)      #倍数
t_total = 1400           # 时间总量300s
cal_finaltime=t_total       #计算时长
sequen=cal_finaltime*times    #数组长度

num_sections = 223      # 节数
length_head = 3.41      # 头部长度
length_body = 2.20      # 身体部分长度
to_wid_distance=0.275      #把手到宽距离
wid_distance=0.3            #宽度
xieian_distance=np.sqrt(to_wid_distance**2+wid_distance**2)
tan_xiebian=6.0/11.0
xiebian_theta=np.arctan(tan_xiebian)

sections_lengths = [length_head] + [length_body] * (num_sections - 1)       #存储板凳长度
positions = np.zeros((sequen+times, num_sections+1,4))      #r,theta,x,y
velocities = np.zeros((sequen + times, num_sections))       #v


#R=A+B*THETA
a=p*16
b=p/2/np.pi

def plot_positions():
    fig, ax = plt.subplots(figsize=(10, 10))
    ax.set_aspect('equal')
    for t in [0*times, 60*times, 120*times, 180*times, 240*times, 300*times]:
        ax.plot(positions[t, :,2], positions[t, :,3], label=f't={t}s')
    ax.scatter(positions[:, 0,2], positions[:, 0,3], color='red', s=50, label='trace of  loong head')
    ax.set_title('the trace of the team')
    ax.set_xlabel('x_position(m)')
    ax.set_ylabel('y_position (m)')
    ax.legend()
    plt.grid(True)
    plt.show()

def write_excel(times_index):
    result = pd.DataFrame(columns=["time", "section", "x_position", "y_position", "r_","theta","velocity"])
    data = []
    for j in range(num_sections):
        i = int(times_index * times)
        data.append({
            "time": i,
            'section': j + 1,
            'x_position': positions[i, j, 2],
            'y_position': positions[i, j, 3],
            "r_":positions[i,j,0],
            "theta" :positions[i,j,1],
            'velocity': velocities[i, j]
        })
    print(1)
    result = pd.DataFrame(data)
    result.to_excel('result2.xlsx', index=False)

def write_excel():
    result = pd.DataFrame(columns=["time", "section", "x_position", "y_position", "r_","theta","velocity"])
    data = []

    for k in np.arange(0, cal_finaltime, 1):
        for j in range(num_sections):
            i = int(k * times)
            data.append({
                "time": i,
                'section': j + 1,
                'x_position': positions[i, j, 2],
                'y_position': positions[i, j, 3],
                "r_":positions[i,j,0],
                "theta" :positions[i,j,1],
                'velocity': velocities[i, j]
            })
    print(1)
    result = pd.DataFrame(data)
    result.to_excel('result.xlsx', index=False)


def calculate_R(theta):
    return a+b*theta
def x_y_cal(t,section_index):
    bu_jin=int(t*times)
    r=positions[bu_jin,section_index,0]
    theta=positions[bu_jin,section_index,1]
    x,y=r*np.cos(theta),r*np.sin(theta)
    return x,y

def calculate_position(t, section_index):
    index=int(t*times)
    r=0
    theta=0
    if section_index == 0:#公式推到
        theta=(np.sqrt(a**2/b+2*t)+a/np.sqrt(b))/np.sqrt(b)*(-1)+(2*a/b)
        r = calculate_R(theta)
    else:
        prev_r, prev_t = positions[index, section_index - 1,0],positions[index, section_index - 1,1]#由前一节点，板凳长度推导
        d= sections_lengths[section_index-1] - 2 * to_wid_distance
        theta=prev_t+np.sqrt(d**2/(1+(prev_r**2))*b**2)         #公式误差待改进
        r=calculate_R(theta)
        if r>a:
             r=theta=0
    if(r<=0):
        r=theta=0
    return r, theta

def calculate_velocity(t, section_index):
    index=int(t*times)
    # if index <3:
    #     return 0
    # now_r = positions[index, section_index,0]
    # prev_t = positions[index-1, section_index,1]
    # now_t=positions[index, section_index,1]
    # v_ = (now_t-prev_t)/det_t*np.sqrt(b**2+now_r**2)

    prev_x,prev_y=positions[index-1,section_index,2],positions[index-1,section_index,3]
    if(prev_y==0 and prev_x==0):
        return 0
    now_x,now_y=positions[index,section_index,2],positions[index,section_index,3]
    v_=np.sqrt((now_x-prev_x)**2+(now_y-prev_y)**2)/det_t
    # v_, error = quad(calculate_R, prev_x, now_x)
    # v_=v_/2/det_t
    return v_


def line_equation_from_two_points(x1, y1, x2, y2):
    if x2 - x1 == 0:
        a = 1
        b = 0
        c = -x1
    elif y2 - y1 == 0:
        a = 0
        b = 1
        c = -y1
    else:
        a = y2 - y1
        b = -(x2 - x1)
        c = x2 * y1 - y2 * x1

    # 可以进一步简化系数，使其更易于阅读
    common_factor = abs(min(a, b, c))
    if common_factor != 0:
        a //= common_factor
        b //= common_factor
        c //= common_factor

    return a, b, c

def distance_point_to_line(x1, y1, a,b,c):
    return np.abs(a*x1+b*y1+c)/np.sqrt(a*a+b*b)

def stop_cal():
    for t in np.arange(0,cal_finaltime,det_t):
        for i in range(num_sections):
            index_arr=int(t*times)

            r_t_temp=calculate_position(t, i)
            positions[index_arr, i, 0], positions[index_arr, i, 1] = r_t_temp[0], r_t_temp[1]

            x_y_temp=x_y_cal(t,i)
            positions[index_arr,i,2],positions[index_arr,i,3]=x_y_temp[0],x_y_temp[1]

            velocities[index_arr, i] = calculate_velocity(t, i)

            if( t>500 and i==0):
                # 龙头前后把手坐标
                x_head_front_1,y_head_front_1=positions[index_arr-times,i,2],positions[index_arr-times,i,3]
                x_head_back_2,y_head_back_2=positions[index_arr-times,i+1,2],positions[index_arr-times,i+1,3]

                #求角度
                slope_head=(y_head_front_1-y_head_back_2)/(x_head_front_1-x_head_back_2)
                theta_head_origin=np.arctan(slope_head)

                # 计算外侧两点的坐标
                head_front_x,head_front_y=x_head_front_1+xieian_distance*np.cos(theta_head_origin+xiebian_theta),y_head_front_1+xieian_distance*np.sin(theta_head_origin+xiebian_theta),
                head_back_x,head_back_y=x_head_back_2+xieian_distance*np.cos(theta_head_origin+np.pi-xiebian_theta),y_head_back_2+xieian_distance*np.sin(theta_head_origin+np.pi-xiebian_theta)

                for j in range(1,10):
                    # 求直线表达式

                    a,b,c=line_equation_from_two_points(positions[index_arr-times,i+j,2],positions[index_arr-times,i+j,3],positions[index_arr-times,i+j+1,2],positions[index_arr-times,i+j+1,3])
                    #计算点到直线距离
                    if distance_point_to_line(head_front_x,head_front_y,a,b,c)<=0.15 or distance_point_to_line(head_back_x,head_back_y,a,b,c)<=0.15:
                        return index_arr

a=stop_cal()
#plot_positions()
#print(positions[-10,0,2],positions[-10,0,3])
print(a)
#write_excel()
